#pragma once
#include <iostream>
#include <string>
#include <vector>

class FrenetPath
{
public:
    // Frenet坐标系下的属性
    std::vector<double> t;     // time
    std::vector<double> d;     // lateral offset
    std::vector<double> d_d;   // lateral speed
    std::vector<double> d_dd;  // lateral acceleration
    std::vector<double> d_ddd; // lateral jerk
    std::vector<double> s;     // s position along spline
    std::vector<double> s_d;   // s speed
    std::vector<double> s_dd;  // s acceleration
    std::vector<double> s_ddd; // s jerk

    // 世界坐标系下的参数
    std::vector<double> x;     // x position
    std::vector<double> y;     // y position
    std::vector<double> theta; // yaw in rad
    std::vector<double> kappas;              // curvature曲率
    std::vector<double> dL;    // Running length / arc length
    std::vector<double> v;     // Tangential velocity
    std::vector<double> a;     // Tangential acceleration

    // Debug
    // std::vector<double> ix;
    // std::vector<double> iy;
    // std::vector<double> iyaw;

    // 横向代价属性
    double c_lateral_deviation;
    double c_lateral_velocity;
    double c_lateral_acceleration;
    double c_lateral_jerk;
    double wd; // Lateral cost
    double Jd; // Lateral cost
    // 纵向代价属性
    double c_longitudinal_acceleration;
    double c_longitudinal_jerk;
    double c_time_taken;
    double c_end_speed_deviation;
    double ws; // Longitudinal cost
    // 总的代价函数
    double W; // Total cost
    double Js; // Longitudinal cost
    // 总的代价函数
    double J; // Total cost
};